#include "flash.h"
#include <string.h>
#include <stdio.h>

// 全局配置变量
// static uint8_t g_verify_after_write = 0; // 在写入后校验数据

/**
 * @brief  设置是否在写入后校验数据
 * @param  verify: 0-不校验, 1-校验
 * @retval 无
 */
// void FLASH_SetVerifyMode(uint8_t verify)
// {
//     g_verify_after_write = verify ? 1 : 0;
// }

/*Flash初始化*/
void FLASH_Init(void)
{
    HAL_FLASH_Unlock();
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);
    HAL_FLASH_Lock();
    // 关闭写保护
}


/**
 * @brief  根据地址计算所在页码
 * @param  addr: Flash地址
 * @retval 页码
 */
uint32_t FLASH_GetPage(uint32_t addr)
{
    return (addr - FLASH_BASE_ADDR) / FLASH_PAGE_SIZE;
}

/**
 * @brief  根据地址计算Bank
 * @param  addr: Flash地址
 * @retval Bank号
 */
uint32_t FLASH_GetBank(uint32_t addr)
{
    return FLASH_BANK_1; // STM32G431KB只有一个Bank
}

/**
 * @brief  擦除指定页
 * @param  page: 页码 (0-63)
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_ErasePage(uint32_t page)
{
    return FLASH_ErasePages(page, 1);
}

/**
 * @brief  擦除多个页
 * @param  start_page: 起始页码
 * @param  nb_pages: 要擦除的页数
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_ErasePages(uint32_t start_page, uint32_t nb_pages)
{
    FLASH_EraseInitTypeDef EraseInitStruct;
    uint32_t page_error = 0;
    HAL_StatusTypeDef hal_status;

    // 检查参数
    if (start_page + nb_pages > FLASH_TOTAL_PAGES)
    {
        return FLASH_ERROR_PARAM;
    }

    // 解锁Flash
    if (HAL_FLASH_Unlock() != HAL_OK)
    {
        return FLASH_ERROR_BUSY;
    }

    // 清除所有错误标志
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);

    // 配置擦除结构体
    EraseInitStruct.TypeErase = FLASH_TYPEERASE_PAGES;
    EraseInitStruct.Banks = FLASH_BANK_1;
    EraseInitStruct.Page = start_page;
    EraseInitStruct.NbPages = nb_pages;

    // 关闭中断并执行擦除
    uint32_t primask = __get_PRIMASK();
    __disable_irq();
    hal_status = HAL_FLASHEx_Erase(&EraseInitStruct, &page_error);
    // 上锁Flash
    HAL_FLASH_Lock();
    if (!primask)
        __enable_irq();
        
    // 返回状态
    if (hal_status != HAL_OK)
    {
        return (hal_status == HAL_TIMEOUT) ? FLASH_ERROR_TIMEOUT : (hal_status == HAL_BUSY) ? FLASH_ERROR_BUSY
                                                                                            : FLASH_ERROR_ERASE;
    }

    return FLASH_OK;
}

/**
 * @brief  向Flash写入数据(双字，64位，8BYTE)
 * @param  address: 写入地址 (必须是8字节对齐)
 * @param  data: 待写入数据指针
 * @param  length: 数据长度 (单位: 8字节)
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_Write_2wordmode(uint32_t address, uint8_t *pdata, uint16_t length)
{
    printf("FLASH_Write called: address=0x%08X, length=%d\r\n", address, length);
    HAL_StatusTypeDef hal_status = HAL_OK;
    // 基本参数检查
    if (pdata == NULL || length == 0 || address % 8 != 0 ||
        address < FLASH_BASE_ADDR ||
        address + length  > FLASH_BASE_ADDR + FLASH_PAGE_SIZE * FLASH_TOTAL_PAGES)
    {
        printf("FLASH_Write_2wordmode: Invalid parameter\r\n");
        return FLASH_ERROR_PARAM;
    }

    // 解锁Flash
    if (HAL_FLASH_Unlock() != HAL_OK)
    {
        return FLASH_ERROR_BUSY;
    }

    // 清除所有错误标志
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);

    // 关闭中断并写入数据
    uint32_t primask = __get_PRIMASK();
    __disable_irq();
    printf("begin write\r\n");
    // 逐个双字写入
    for (uint32_t i = 0; i < length && hal_status == HAL_OK; i+=8)
    {
        // printf("i = %d\r\n", i);
        // uint64_t tmpread = *(uint64_t *)(address + i);
        // printf("read 0x%08X: 0x%016llX\r\n", address + i, tmpread);
        // printf("write 0x%08X: 0x%016llX\r\n", address + i, *(uint64_t *)(pdata+i));
        hal_status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address + i, *(uint64_t *)(pdata+i));
    }

    // 恢复中断状态
    if (!primask)
        __enable_irq();

    // 上锁Flash
    HAL_FLASH_Lock();

    // 处理错误
    if (hal_status != HAL_OK)
    {
        return (hal_status == HAL_TIMEOUT) ? FLASH_ERROR_TIMEOUT : (hal_status == HAL_BUSY) ? FLASH_ERROR_BUSY
                                                                                            : FLASH_ERROR_WRITE;
    }

    return FLASH_OK;
}

/**
 * @brief  向Flash写入数据(快速模式)
 * @param  address: 写入地址 (必须是256字节对齐)
 * @param  data:    待写入数据指针，必须指向至少的数据
 * @param  length:  数据长度 (单位: BYTE)
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_Write_fast(uint32_t address,
                                     uint8_t *data,
                                     uint32_t length)
{
     printf("FLASH_Write called: address=0x%08X, length=%d\r\n", address, length);
    HAL_StatusTypeDef hal_status = HAL_OK;

    /* 基本参数检查 */
    if (data == NULL || length == 0 ||
        address % 256 != 0 ||
        address < FLASH_BASE_ADDR ||
        address + length * 8 > FLASH_BASE_ADDR + FLASH_PAGE_SIZE * FLASH_TOTAL_PAGES)
    {
        return FLASH_ERROR_PARAM;
    }

    /* 解锁 Flash */
    if (HAL_FLASH_Unlock() != HAL_OK)
        return FLASH_ERROR_BUSY;

    /* 清除所有错误标志 */
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS);

    /* 关中断 */
    uint32_t primask = __get_PRIMASK();
    __disable_irq();

    /* 逐块写入 2048 字节 */
    uint32_t turn = (length+255) / 256;
    for (uint32_t i = 0; i < turn && hal_status == HAL_OK; ++i)
    {
        /* 计算当前块的首地址 */
        uint32_t block_addr = address + i * 256;

        /* 决定本次使用的类型 */
        uint32_t program_type =
            (i == turn - 1) ? FLASH_TYPEPROGRAM_FAST_AND_LAST
                              : FLASH_TYPEPROGRAM_FAST;
        /* 写入数据 */
        hal_status = HAL_FLASH_Program(program_type,
                                       block_addr,
                                       (uint64_t)(data+i*256));
        // HAL_Delay(5);
    }
    /* 恢复中断 */
    if (!primask) __enable_irq();
    /* 上锁 Flash */
    HAL_FLASH_Lock();

    /* 错误处理 */
    if (hal_status != HAL_OK)
    {
        return (hal_status == HAL_TIMEOUT) ? FLASH_ERROR_TIMEOUT
             : (hal_status == HAL_BUSY)    ? FLASH_ERROR_BUSY
                                           : FLASH_ERROR_WRITE;
    }

    return FLASH_OK;
}

/**
 * @brief  从Flash读取数据
 * @param  address: 读取地址
 * @param  data: 数据存储指针 (64位双字数据)
 * @param  length: 数据长度 (单位: 8字节)
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_ReadData(uint32_t address, uint32_t *data, uint32_t length)
{
    // 基本参数检查
    if (data == NULL || length == 0 ||
        address < FLASH_BASE_ADDR ||
        address + length * 4 > FLASH_BASE_ADDR + FLASH_PAGE_SIZE * FLASH_TOTAL_PAGES)
    {
        return FLASH_ERROR_PARAM;
    }

    // 批量将内存拷贝
    memcpy(data, (void *)address, length * 4);
    return FLASH_OK;
}

FLASH_StatusTypeDef FLASH_ReadData_Bytes(uint32_t address, uint8_t *data, uint32_t length){
    // 批量将内存拷贝
    memcpy(data, (void *)address, length);
    return FLASH_OK;
}

/**
 * @brief  校验Flash数据
 * @param  address: 校验地址
 * @param  data: 参考数据指针
 * @param  length: 数据长度 (单位: 8字节)
 * @retval FLASH_StatusTypeDef: 操作状态
 */
FLASH_StatusTypeDef FLASH_VerifyData(uint32_t address, uint32_t *data, uint32_t length)
{
    // 基本参数检查
    if (data == NULL || length == 0 ||
        address < FLASH_BASE_ADDR ||
        address + length * 4 > FLASH_BASE_ADDR + FLASH_PAGE_SIZE * FLASH_TOTAL_PAGES)
    {
        return FLASH_ERROR_PARAM;
    }

    // 校验数据
    uint32_t *p_flash = (uint32_t *)address;
    for (uint32_t i = 0; i < length; i++)
    {
        if (p_flash[i] != data[i])
        {
            return FLASH_ERROR_VERIFY;
        }
    }

    return FLASH_OK;
}
/*
 * @brief  将8个字节打包为64位无符号整数
 * @param  src: 输入字节数组指针
 * @param  dst: 输出64位无符号整数指针
 * @param  len: 输入字节数组长度 (必须为8的倍数)
 * @retval 无
 */
void pack_uint8_to_uint64(const uint8_t *src, uint64_t *dst, size_t len) {
    for (size_t i = 0; i < len; i += 8) {
        uint64_t val = 0;
        for (int j = 0; j < 8; ++j) {
            val |= ((uint64_t)(i + j < len ? src[i + j] : 0xFF)) << (8 * j);
        }
        dst[i / 8] = val;
    }
}

// // 测试函数
// void FLASH_Test_2words(void)
// {
//     // 双字擦写读测试
//     printf("double word test\r\n");
//     FLASH_ErasePage(12);
//     uint32_t addr_2word = 0x08006000;
//     uint64_t writeData = 0x1122334455667788;
//     FLASH_StatusTypeDef status1 = FLASH_Write_2wordmode(addr_2word, &writeData, 1);
//     printf("write status:%d\r\n", status1);
//     uint64_t readData = 0;
//     // FLASH_ReadData_2word(addr_2word, &readData, 1);
//     printf("at address:0x%x, read value:0x%llx\r\n", addr_2word, readData);
// }

// void FLASH_Test_1word(void)
// {
//     // 单字擦写读测试
//     printf("single word test\r\n");
//     FLASH_ErasePage(12);
//     uint32_t addr_2word = 0x08006000;
//     uint64_t writeData = 0x55555555;
//     FLASH_StatusTypeDef status1 = FLASH_Write_2wordmode(addr_2word, &writeData, 1);
//     printf("write status:%d\r\n", status1);
//     uint32_t readData = 0;
//     FLASH_ReadData(addr_2word, &readData, 1);
//     printf("at address:0x%x, read value:0x%x\r\n", addr_2word, readData);
// }
